/*
 * @Description: session data reactor
 * @Version: V1.0
 * @Author: zw_1520@163.com
 * @Date: 2024-09-09 13:53:47
 * @LastEditors: zw_1520@163.com
 * @LastEditTime: 2025-04-22 14:01:17
 * Copyright (C) All rights reserved.
 */
#ifndef __STREAM_DATA_REACTOR_H__
#define __STREAM_DATA_REACTOR_H__

#include <grpcpp/ext/proto_server_reflection_plugin.h>
#include <grpcpp/grpcpp.h>
#include <grpcpp/health_check_service_interface.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <iostream>
#include <memory>
#include <queue>
#include <rclcpp/rclcpp.hpp>
#include <robot_interface/msg/robot_task_info.hpp>
#include <robot_interface/srv/set_robot_task.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/string.hpp>
#include <string>

#include "absl/flags/flag.h"
#include "absl/flags/parse.h"
#include "absl/strings/str_format.h"
#include "robot_grpc_proto/robot.grpc.pb.h"

using grpc::CallbackServerContext;
using grpc::Server;
using grpc::ServerBidiReactor;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::ServerReaderWriter;
using grpc::Status;
using robot::RobotController;
using robot::RobotRequest;
using robot::RobotResponse;

#define PI (3.14159)

class RobotControllerServiceImpl final : public RobotController::Service {
 public:
  RobotControllerServiceImpl()
      : node_(rclcpp::Node::make_shared("robot_grpc_service")) {
    // Inits tf buffer and listener
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    // Inits hand joint publisher
    left_dexterous_hand_joint_publisher_ =
        node_->create_publisher<sensor_msgs::msg::JointState>(
            "/left_hand/robot_hand_target_joint_state", 10);
    right_dexterous_hand_joint_publisher_ =
        node_->create_publisher<sensor_msgs::msg::JointState>(
            "/right_hand/robot_hand_target_joint_state", 10);

    agibot_task_contorl_client_ =
        node_->create_client<robot_interface::srv::SetRobotTask>(
            "/agibot_control_service");

    agibot_task_control_publisher_ =
        node_->create_publisher<robot_interface::msg::RobotTaskInfo>(
            "/agibot_task_control", 10);

    RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                "Grpc Server init success.");
    std::thread robot_control_msg_processs_thread(
        &RobotControllerServiceImpl::requestProcessThreadEntry, this);
    robot_control_msg_processs_thread.detach();
  }

  Status RobotControlStream(
      ServerContext* context,
      ServerReaderWriter<RobotResponse, RobotRequest>* stream) override {
    // Creates a thread for sending message
    writer_stop_ = false;
    std::thread writer([context, stream, this]() {
      while (!writer_stop_) {
        RobotResponse response;
        if (!reply_queue_.empty()) {
          RobotResponse robot_reply;
          {
            // Gets lock to pop request data
            std::lock_guard<std::mutex> locker(this->reply_queue_mutex_);
            RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                        "get reply_queue size before pop: %ld, empty: %d.",
                        this->reply_queue_.size(), reply_queue_.empty());
            if (this->reply_queue_.size() > 0) {
              robot_reply = this->reply_queue_.front();
              this->reply_queue_.pop();
            } else {
              std::this_thread::sleep_for(std::chrono::milliseconds(1));
              continue;
            }
            // Processes request
            RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                        "reply_queue size after: %ld, empty: %d.",
                        this->reply_queue_.size(), reply_queue_.empty());
          }
          {
            RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                        "Send response: %s.", robot_reply.msg_id().c_str());
            RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                        "Send response: %s.", robot_reply.data().c_str());
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
            static int msg_count = 1;
            if (writer_stop_) {
              RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                          "Client has disconnected.");
            } else {
              stream->Write(robot_reply);

              RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                          "Send response: %s finished, msg_count: %d.",
                          robot_reply.msg_id().c_str(), msg_count++);
            }
          }
        }
        // Sleeps for lower cpu usage
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
      }
      RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                  "Server closed the write stream.");
    });

    // Starts to receive message
    RobotRequest request;
    while (stream->Read(&request)) {
      static int msg_count = 1;
      // Process the incoming request
      RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                  "Received msg_id: %s, msg_count: %d.",
                  request.msg_id().c_str(), msg_count++);
      RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                  "Received data: %s", request.data().c_str());
      // Adds request data to queue
      {
        std::lock_guard<std::mutex> locker(request_queue_mutex_);
        request_queue_.push(request);
      }
    }
    writer_stop_ = true;
    writer.join();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    if (context->IsCancelled()) {
      RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                  "Read close stream, stream has disconnected.");
    }
    RCLCPP_INFO(rclcpp::get_logger("stream_data_reactor"),
                "Server closed the read stream.");
    return Status::OK;
  }

  // Provides for external module to send data
  void sendData(const RobotResponse& data);

  // Defines request process thread entry function
  void requestProcessThreadEntry();

  // Processes request
  void processRequest(RobotRequest& robot_request);

  // Processes motion request message
  bool taskControl(std::string msg);

  // Processes motion playing result
  void playMotionResponse(
      rclcpp::Client<robot_interface::srv::SetRobotTask>::SharedFuture future);

 private:
  rclcpp::Node::SharedPtr node_;
  CallbackServerContext* context_;
  RobotRequest request_;
  RobotResponse response_;
  std::queue<RobotRequest> request_queue_;  // Saves request data
  std::queue<RobotResponse> reply_queue_;   // Saves response data
  static RobotControllerServiceImpl* current_instance_;
  std::mutex request_queue_mutex_;
  std::mutex reply_queue_mutex_;
  bool writer_stop_ = false;

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  // Defines robot dexterous hand control interface
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr
      left_dexterous_hand_joint_publisher_;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr
      right_dexterous_hand_joint_publisher_;

  rclcpp::Client<robot_interface::srv::SetRobotTask>::SharedPtr
      agibot_task_contorl_client_;
  rclcpp::Publisher<robot_interface::msg::RobotTaskInfo>::SharedPtr
      agibot_task_control_publisher_;
};

#endif